% 我自己进行标定的（a,d只有正，theta、alpha顺负逆正）
% 且该程序的关节角与南楼机器人上位机显示角度的关系为：theta1=theta1;theta2=-(theta2+90°);theta3=-theta3;theta4=theta4+90°;
% theta5=theta5-180°;theta6=theta6
% clear;
% clc;
% L(1)=Link('revolute','d',113.9,'a',0,'alpha',0,'modified');
% L(2)=Link('revolute','d',0,'a',0,'alpha',-pi/2,'offset',-pi/2,'modified');
% L(3)=Link('revolute','d',0,'a',351,'alpha',0,'modified');
% L(4)=Link('revolute','d',102.3,'a',334.5,'alpha',0,'offset',pi/2,'modified');
% L(5)=Link('revolute','d',102.3,'a',0,'alpha',pi/2,'modified');
% L(6)=Link('revolute','d',56.8,'a',0,'alpha',-pi/2,'modified');
% Six_dof=SerialLink(L,'name','6-dof');
% Six_dof.teach
% 南楼越疆机器人DH参数
L(1)=Link('revolute','d',0,'a',0,'alpha',0,'modified');
L(2)=Link('revolute','d',0,'a',0,'alpha',pi/2,'offset',pi/2,'modified');
L(3)=Link('revolute','d',0,'a',427,'alpha',0,'modified');
L(4)=Link('revolute','d',116,'a',357,'alpha',0,'offset',pi/2,'modified');
L(5)=Link('revolute','d',116,'a',0,'alpha',pi/2,'modified');
L(6)=Link('revolute','d',105,'a',0,'alpha',-pi/2,'modified');
Six_dof=SerialLink(L,'name','6-dof');
Angleof6=[%-23.788,-9.154,105.111,-4.492,-72.318,60.015;
          -21.07,-9.154,104.123,-3.59,-73.88,58.044;
          -19.565,-8.803,103.917,-4.283,-74.546,50.751;
          %-18.088,-7.49,102.348,-3.098,-75.185,46.692;
          %-16.41,-5.72,99.959,-1.456,-73.652,49.448;
          %-14.403,-4.225,98.025,-3.134,-76.553,41.295;
          -11.193,-6.815,100.488,-1.328,-80.146,44.852;
          -9.114,-4.517,97.97,0.842,-84.14,49.248;
          -5.994,-2.711,91.149,-1.037,-87.332,44.742;
          -1.909,-4.59,96.174,1.681,-91.296,50.725;
          3.49,-3.477,90.237,-0.106,-94.561,57.839;
          6.992,-1.304,87.884,2.52,-96.804,60.775;
          9.874,2.381,85.038,6.843,-93.704,55.741;
          13.449,0.338,86.88,1.627,-98.61,52.119;
          21.809,4.241,85.311,-0.261,-102.623,72.228];  %机器人关节角
%Six_dof.fkine()
Six_dof.teach
% 经过激光跟踪仪修改后的DH参数

% 原有的
% clear;
% clc;
% L(1)=Link('revolute','d',113.9,'a',0,'alpha',0,'modified');
% L(2)=Link('revolute','d',0,'a',0,'alpha',pi/2,'offset',-pi/2,'modified');
% L(3)=Link('revolute','d',0,'a',-351,'alpha',0,'modified');
% L(4)=Link('revolute','d',102.3,'a',-334.5,'alpha',0,'offset',-pi/2,'modified');
% L(5)=Link('revolute','d',102.3,'a',0,'alpha',pi/2,'modified');
% L(6)=Link('revolute','d',56.8,'a',0,'alpha',-pi/2,'modified');
% Six_dof=SerialLink(L,'name','6-dof');
% Six_dof.teach
% 工具坐标系
% clear;
% clc;
% L(1)=Link('revolute','d',113.9,'a',0,'alpha',0,'modified');
% L(2)=Link('revolute','d',0,'a',0,'alpha',-pi/2,'offset',-pi/2,'modified');
% L(3)=Link('revolute','d',0,'a',352.1,'alpha',0,'modified');
% L(4)=Link('revolute','d',105.3,'a',335.6,'alpha',0,'offset',pi/2,'modified');
% L(5)=Link('revolute','d',103.1,'a',0,'alpha',pi/2,'modified');
% L(6)=Link('revolute','d',56.8,'a',0,'alpha',-pi/2,'offset',-1.3725,'modified');
% L(7)=Link('revolute','d',64.5649,'a',118.7229,'alpha',0,'modified');
% Seven_dof=SerialLink(L,'name','7-dof');
% Seven_dof.teach
%激光跟踪仪算的角度是58.0698°，弧度制为1.0135.